//#include <math.h>
//#include <stdint.h>
//#include "stm32f10x.h"                  // Device header
//#include "Filter.h"
//#include "OLED.h"
//#include <math.h>
// uint16_t tau1;
// uint16_t tau2;
// uint16_t tau3;
//double tdoaLsSolve( float tdoa1 , float tdoa2 , float tdoa3 , float* result)
//	{
//    double  Distance = 0.0f;
//     float x1 = 0.0f;
//    float y1 = 1.0f;
//    float x2 = 3.0f;
//    float y2 = 0.0f;
//    float x3 = -2.0f;
//    float y3 = 0.0f;
//    float x4 = 0.0f;
//    float y4 = -1.0f;
//   
//  
//    float R2_1 = 343 * (tdoa1);
//    float R3_1 = 343 * (tdoa2);
//    float R4_1 = 343 * (tdoa3);
//    

//    float A[3][3] = 
//		{
//        {x2 - x1, y2 - y1, R2_1},
//        {x3 - x1, y3 - y1, R3_1},
//        {x4 - x1, y4 - y1, R4_1}  //参考资料：知乎（见学习日记）
//    };
//    
//    // 计算K值
//    float K1 = x1 * x1 + y1 * y1;
//    float K2 = x2 * x2 + y2 * y2;
//    float K3 = x3 * x3 + y3 * y3;
//    float K4 = x4 * x4 + y4 * y4;
//    
//    
//    float b[3] = 
//		{
//        0.5f * (K2 - K1 - R2_1 * R2_1),
//        0.5f * (K3 - K1 - R3_1 * R3_1),
//        0.5f * (K4 - K1 - R4_1 * R4_1)
//    };
//		
//    float ATA[3][3] = {0};
//    for (int i = 0; i < 3; i++) 
//		{
//        for (int j = 0; j < 3; j++) 
//			{
//            for (int k = 0; k < 3; k++) 
//				    {
//                ATA[i][j] += A[k][i] * A[k][j];
//            }
//      }
//    }                      
//    
//    // 计算A'*b (ATb)
//    float ATb[3] = {0};
//    for (int i = 0; i < 3; i++) 
//		{
//        for (int k = 0; k < 3; k++) 
//			  {
//            ATb[i] += A[k][i] * b[k];
//        }                               //矩阵*向量
//    }
//    
//    // 计算ATA的逆矩阵并求解X = (ATA)^-1 * ATb
//    // 3x3矩阵求逆简化实现
//    float det = ATA[0][0] * (ATA[1][1] * ATA[2][2] - ATA[1][2] * ATA[2][1])
//              - ATA[0][1] * (ATA[1][0] * ATA[2][2] - ATA[1][2] * ATA[2][0])
//              + ATA[0][2] * (ATA[1][0] * ATA[2][1] - ATA[1][1] * ATA[2][0]);
//              
//    if (fabs(det) < 1e-9f) 
//		{
//                                                                                // 判断是否矩阵奇异 （豆包）
//        result[0] = 0.0f;
//        result[1] = 0.0f;
//        OLED_ShowString (4,1,"Faulse");
//    }
//    
//    float invATA[3][3];
//    invATA[0][0] = (ATA[1][1] * ATA[2][2] - ATA[1][2] * ATA[2][1]) / det;
//    invATA[0][1] = (ATA[0][2] * ATA[2][1] - ATA[0][1] * ATA[2][2]) / det;
//    invATA[0][2] = (ATA[0][1] * ATA[1][2] - ATA[0][2] * ATA[1][1]) / det;
//    invATA[1][0] = (ATA[1][2] * ATA[2][0] - ATA[1][0] * ATA[2][2]) / det;
//    invATA[1][1] = (ATA[0][0] * ATA[2][2] - ATA[0][2] * ATA[2][0]) / det;
//    invATA[1][2] = (ATA[0][2] * ATA[1][0] - ATA[0][0] * ATA[1][2]) / det;
//    invATA[2][0] = (ATA[1][0] * ATA[2][1] - ATA[1][1] * ATA[2][0]) / det;
//    invATA[2][1] = (ATA[0][1] * ATA[2][0] - ATA[0][0] * ATA[2][1]) / det;
//    invATA[2][2] = (ATA[0][0] * ATA[1][1] - ATA[0][1] * ATA[1][0]) / det;
//    
//                                                                                          // 计算结果 X = invATA * ATb
//    result[0] = invATA[0][0] * ATb[0] + invATA[0][1] * ATb[1] + invATA[0][2] * ATb[2];   //横轴
//    result[1] = invATA[1][0] * ATb[0] + invATA[1][1] * ATb[1] + invATA[1][2] * ATb[2];   //纵轴
//		uint32_t x = result[0] * result[0] + result[1] * result[1] ;
//		Distance = sqrt((double) x) ;
//		return Distance ;
//}

#include <math.h>
#include <stdint.h>
#include "stm32f10x.h"                  // Device header
#include "Filter.h"
#include "OLED.h"
#include <math.h>

uint16_t tau1;
uint16_t tau2;
uint16_t tau3;

#define SOUND_SPEED 343.0f

/**
 * @brief 标准Chan氏算法解算TDOA定位（适配原有代码框架）
 * @param tdoa1 锚点2与锚点1的时间差
 * @param tdoa2 锚点3与锚点1的时间差
 * @param tdoa3 锚点4与锚点1的时间差
 * @param result 输出定位结果，result[0]为x轴，result[1]为y轴
 * @return 定位点到原点的距离
 */
double tdoaLsSolve(float tdoa1, float tdoa2, float tdoa3, float* result) {
    double Distance = 0.0f;
    // 锚点坐标（完全沿用原有代码中的固定值）
    float x1 = 0.0f;
    float y1 = 1.0f;
    float x2 = 3.0f;
    float y2 = 0.0f;
    float x3 = -2.0f;
    float y3 = 0.0f;
    float x4 = 0.0f;
    float y4 = -1.0f;

    // 计算距离差（以锚点1为参考，对应tdoa1/2/3）
    float dr1 = SOUND_SPEED * tdoa1;  // 锚点2-锚点1的距离差
    float dr2 = SOUND_SPEED * tdoa2;  // 锚点3-锚点1的距离差
    float dr3 = SOUND_SPEED * tdoa3;  // 锚点4-锚点1的距离差

    // 计算各锚点到原点的距离（Chan氏算法第一步所需中间量）
    float d1 = sqrt(x1 * x1 + y1 * y1);
    float d2 = sqrt(x2 * x2 + y2 * y2);
    float d3 = sqrt(x3 * x3 + y3 * y3);
    float d4 = sqrt(x4 * x4 + y4 * y4);

    // 计算线性化方程的常数项（Chan氏算法第一步）
    float l1 = (d2 * d2 - d1 * d1 - dr1 * dr1) / 2.0f;
    float l2 = (d3 * d3 - d1 * d1 - dr2 * dr2) / 2.0f;
    float l3 = (d4 * d4 - d1 * d1 - dr3 * dr3) / 2.0f;

    // 构建观测矩阵A（3行2列，超定方程组）
    float A[3][2] = {
        {x2 - x1, y2 - y1},
        {x3 - x1, y3 - y1},
        {x4 - x1, y4 - y1}
    };

    // 构建距离差向量Dr和常数项向量D
    float Dr[3] = {-dr1, -dr2, -dr3};
    float D[3] = {l1, l2, l3};

    // 计算A^T*A（最小二乘求解所需）
    float AtA[2][2] = {0};
    for (int i = 0; i < 2; i++) {
        for (int j = 0; j < 2; j++) {
            for (int k = 0; k < 3; k++) {
                AtA[i][j] += A[k][i] * A[k][j];
            }
        }
    }

    // 计算A^T*Dr 和 A^T*D
    float AtDr[2] = {0};
    float AtD[2] = {0};
    for (int i = 0; i < 2; i++) {
        for (int k = 0; k < 3; k++) {
            AtDr[i] += A[k][i] * Dr[k];
            AtD[i] += A[k][i] * D[k];
        }
    }

    // 计算AtA的逆矩阵（2x2矩阵求逆，避免原代码中3x3矩阵的冗余）
    float det = AtA[0][0] * AtA[1][1] - AtA[0][1] * AtA[1][0];
    // 矩阵奇异判断，无法求解时返回原点并提示
    if (fabs(det) < 1e-9f) {
        result[0] = 0.0f;
        result[1] = 0.0f;
        OLED_ShowString(4, 1, "Faulse");
        return 0.0f;
    }
    // 2x2矩阵逆矩阵公式
    float inv_AtA[2][2] = {
        {AtA[1][1]/det, -AtA[0][1]/det},
        {-AtA[1][0]/det, AtA[0][0]/det}
    };

    // Chan氏算法第一步：求解中间变量a和b
    float a[2] = {
        inv_AtA[0][0] * AtDr[0] + inv_AtA[0][1] * AtDr[1],
        inv_AtA[1][0] * AtDr[0] + inv_AtA[1][1] * AtDr[1]
    };
    float b[2] = {
        inv_AtA[0][0] * AtD[0] + inv_AtA[0][1] * AtD[1],
        inv_AtA[1][0] * AtD[0] + inv_AtA[1][1] * AtD[1]
    };

    // Chan氏算法第二步：求解二次方程（核心步骤，区别于原最小二乘）
    float I = a[0]*a[0] + a[1]*a[1] - 1.0f;
    float J = a[0]*(b[0] - x1) + a[1]*(b[1] - y1);
    float K = (x1 - b[0])*(x1 - b[0]) + (y1 - b[1])*(y1 - b[1]);

    // 判别式计算，无实数解时返回原点并提示
    float sqrt_val = J*J - I*K;
    if (sqrt_val < 0) {
        result[0] = 0.0f;
        result[1] = 0.0f;
        OLED_ShowString(4, 1, "Faulse");
        return 0.0f;
    }
    sqrt_val = sqrt(sqrt_val);

    // 选择物理意义合理的根（根据锚点分布，排除超出合理范围的根）
    float r = -(J + sqrt_val) / I;
    // 若根异常（如距离过远），可切换为另一个根：r = -(J - sqrt_val) / I;

    // 计算最终定位结果
    result[0] = a[0] * r + b[0];  // 横轴x
    result[1] = a[1] * r + b[1];  // 纵轴y

    // 计算定位点到原点的距离（与原代码返回值一致）
    float pos_dist_sq = result[0] * result[0] + result[1] * result[1];
    Distance = sqrt((double)pos_dist_sq);

    return Distance;
}

